#include <QModbusClient>
#include "modbusrtuclient.h"

#if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
#include <QtCore/qmath.h>
#endif

///
/// \brief The QModbusSerialAdu class
///
class QModbusSerialAdu
{
public:
    enum Type {
        Ascii,
        Rtu
    };

    inline QModbusSerialAdu(Type type, const QByteArray &data)
        : _type(type), _data(data), _rawData(data)
    {
        if (_type == Ascii)
            _data = QByteArray::fromHex(_data.mid(1, _data.size() - 3));
    }

    inline int size() const {
        if (_type == Ascii)
            return _data.size() - 1; // one byte, LRC
        return _data.size() - 2; // two bytes, CRC
    }
    inline QByteArray data() const { return _data.left(size()); }

    inline int rawSize() const { return _rawData.size(); }
    inline QByteArray rawData() const { return _rawData; }

    inline int serverAddress() const {
        Q_ASSERT_X(!_data.isEmpty(), "QModbusAdu::serverAddress()", "Empty ADU.");
        return quint8(_data.at(0));
    }

    inline QModbusPdu pdu() const {
        Q_ASSERT_X(!_data.isEmpty(), "QModbusAdu::pdu()", "Empty ADU.");
        return QModbusPdu(QModbusPdu::FunctionCode(_data.at(1)), _data.mid(2, size() - 2));
    }

    template <typename T>
    auto checksum() const -> decltype(T()) {
        Q_ASSERT_X(!_data.isEmpty(), "QModbusAdu::checksum()", "Empty ADU.");
        if (_type == Ascii)
            return quint8(_data[_data.size() - 1]);
        return quint16(quint8(_data[_data.size() - 2]) << 8 | quint8(_data[_data.size() - 1]));
    }

    inline bool matchingChecksum() const {
        Q_ASSERT_X(!_data.isEmpty(), "QModbusAdu::matchingChecksum()", "Empty ADU.");
        if (_type == Ascii)
            return QModbusSerialAdu::calculateLRC(data(), size()) == checksum<quint8>();
        return QModbusSerialAdu::calculateCRC(data(), size()) == checksum<quint16>();
    }

    /*!
        \internal
        \fn quint8 QModbusSerialAdu::calculateLRC(const char *data, qint32 len)

        Returns the LRC checksum of the first \a len bytes of \a data. The checksum is independent
        of the byte order (endianness).
    */
    inline static quint8 calculateLRC(const char *data, qint32 len)
    {
        quint32 lrc = 0;
        while (len--)
            lrc += *data++;
        return -(quint8(lrc));
    }

    /*!
        \internal
        \fn quint16 QModbusSerialAdu::calculateCRC(const char *data, qint32 len) const

        Returns the CRC checksum of the first \a len bytes of \a data.

        \note The code used by the function was generated with pycrc. There is no copyright assigned
        to the generated code, however, the author of the script requests to show the line stating
        that the code was generated by pycrc (see implementation).
    */
    inline static quint16 calculateCRC(const char *data, qint32 len)
    {
        // Generated by pycrc v0.8.3, https://pycrc.org
        // Width = 16, Poly = 0x8005, XorIn = 0xffff, ReflectIn = True,
        // XorOut = 0x0000, ReflectOut = True, Algorithm = bit-by-bit-fast

        quint16 crc = 0xFFFF;
        while (len--) {
            const quint8 c = *data++;
            for (qint32 i = 0x01; i & 0xFF; i <<= 1) {
                bool bit = crc & 0x8000;
                if (c & i)
                    bit = !bit;
                crc <<= 1;
                if (bit)
                    crc ^= 0x8005;
            }
            crc &= 0xFFFF;
        }
        crc = crc_reflect(crc & 0xFFFF, 16) ^ 0x0000;
        return (crc >> 8) | (crc << 8); // swap bytes
    }

    inline static QByteArray create(Type type, int serverAddress, const QModbusPdu &pdu,
                                    char delimiter = '\n') {
        QByteArray result;
        QDataStream out(&result, QIODevice::WriteOnly);
        out << quint8(serverAddress) << pdu;

        if (type == Ascii) {
            out << calculateLRC(result, result.size());
            return ":" + result.toHex() + "\r" + delimiter;
        } else {
            out << calculateCRC(result, result.size());
        }
        return result;
    }

private:
    inline static quint16 crc_reflect(quint16 data, qint32 len)
    {
        // Generated by pycrc v0.8.3, https://pycrc.org
        // Width = 16, Poly = 0x8005, XorIn = 0xffff, ReflectIn = True,
        // XorOut = 0x0000, ReflectOut = True, Algorithm = bit-by-bit-fast

        quint16 ret = data & 0x01;
        for (qint32 i = 1; i < len; i++) {
            data >>= 1;
            ret = (ret << 1) | (data & 0x01);
        }
        return ret;
    }

private:
    Type _type = Rtu;
    QByteArray _data;
    QByteArray _rawData;
};

///
/// \brief ModbusRtuClient::ModbusRtuClient
/// \param parent
///
ModbusRtuClient::ModbusRtuClient(QObject *parent)
    : ModbusClientPrivate{parent}
{
    _serialPort = new QSerialPort(this);

    QObject::connect(&_responseTimer, &Timer::timeout, this, &ModbusRtuClient::on_responseTimeout);
    QObject::connect(_serialPort, &QSerialPort::readyRead, this, &ModbusRtuClient::on_readyRead);
    QObject::connect(_serialPort, &QSerialPort::errorOccurred, this, &ModbusRtuClient::on_error);
    QObject::connect(_serialPort, &QSerialPort::aboutToClose, this, &ModbusRtuClient::on_aboutToClose);
    QObject::connect(_serialPort, &QSerialPort::bytesWritten, this, &ModbusRtuClient::on_bytesWritten);
}

///
/// \brief ModbusRtuClient::~ModbusRtuClient
///
ModbusRtuClient::~ModbusRtuClient()
{
    close();
}

///
/// \brief ModbusRtuClient::isOpen
/// \return
///
bool ModbusRtuClient::isOpen() const
{
    if (_serialPort)
        return _serialPort->isOpen();
    return false;
}

///
/// \brief ModbusRtuClient::connectionParameter
/// \param parameter
/// \return
///
QVariant ModbusRtuClient::connectionParameter(ModbusDevice::ConnectionParameter parameter) const
{
    switch (parameter) {
    case ModbusDevice::SerialPortNameParameter:
        return _comPort;
    case ModbusDevice::SerialDataBitsParameter:
        return _dataBits;
    case ModbusDevice::SerialParityParameter:
        return _parity;
    case ModbusDevice::SerialStopBitsParameter:
        return _stopBits;
    case ModbusDevice::SerialBaudRateParameter:
        return _baudRate;
    default:
        return {};
    }
}

///
/// \brief ModbusRtuClient::setConnectionParameter
/// \param parameter
/// \param value
///
void ModbusRtuClient::setConnectionParameter(ModbusDevice::ConnectionParameter parameter, const QVariant &value)
{
    switch (parameter) {
    case ModbusDevice::SerialPortNameParameter:
        _comPort = value.toString();
        break;
    case ModbusDevice::SerialDataBitsParameter:
        _dataBits = QSerialPort::DataBits(value.toInt());
        break;
    case ModbusDevice::SerialParityParameter:
        _parity = QSerialPort::Parity(value.toInt());
        break;
    case ModbusDevice::SerialStopBitsParameter:
        _stopBits = QSerialPort::StopBits(value.toInt());
        break;
    case ModbusDevice::SerialBaudRateParameter:
        _baudRate = QSerialPort::BaudRate(value.toInt());
        break;
    default:
        Q_ASSERT_X(false, "", "Connection parameter not supported.");
        break;
    }
}

///
/// \brief ModbusRtuClient::interFrameDelay
/// \return
///
int ModbusRtuClient::interFrameDelay() const
{
    return _interFrameDelayMilliseconds * 1000;
}

///
/// \brief ModbusRtuClient::setInterFrameDelay
/// \param microseconds
///
void ModbusRtuClient::setInterFrameDelay(int microseconds)
{
    _interFrameDelayMilliseconds = qCeil(qreal(microseconds) / 1000.);
    calculateInterFrameDelay();
}

///
/// \brief ModbusRtuClient::turnaroundDelay
/// \return
///
int ModbusRtuClient::turnaroundDelay() const
{
    return _turnaroundDelay;
}

///
/// \brief ModbusRtuClient::setTurnaroundDelay
/// \param turnaroundDelay
///
void ModbusRtuClient::setTurnaroundDelay(int turnaroundDelay)
{
    _turnaroundDelay = turnaroundDelay;
}

///
/// \brief ModbusRtuClient::calculateInterFrameDelay
///
void ModbusRtuClient::calculateInterFrameDelay()
{
    // The spec recommends a timeout value of 1.750 msec. Without such
    // precise single-shot timers use a approximated value of 1.750 msec.
    int delayMilliSeconds = RecommendedDelay;
    if (_baudRate < 19200) {
        // Example: 9600 baud, 11 bit per packet -> 872 char/sec so:
        // 1000 ms / 872 char = 1.147 ms/char * 3.5 character = 4.0145 ms
        // Always round up because the spec requests at least 3.5 char.
        delayMilliSeconds = qCeil(3500. / (qreal(_baudRate) / 11.));
    }
    _interFrameDelayMilliseconds = qMax(_interFrameDelayMilliseconds, delayMilliSeconds);
}

///
/// \brief ModbusRtuSerialServer::setupEnvironment
///
void ModbusRtuClient::setupEnvironment()
{
    if (_serialPort) {
        _serialPort->setPortName(_comPort);
        _serialPort->setParity(_parity);
        _serialPort->setBaudRate(_baudRate);
        _serialPort->setDataBits(_dataBits);
        _serialPort->setStopBits(_stopBits);
    }

    calculateInterFrameDelay();

    _responseBuffer.clear();
    _state = ModbusRtuClient::Idle;
}

///
/// \brief ModbusRtuClient::open
/// \return
///
bool ModbusRtuClient::open()
{
    if (state() == ModbusDevice::ConnectedState)
        return true;

    setupEnvironment(); // to be done before open
    if (_serialPort->open(QIODevice::ReadWrite)) {
        setState(ModbusDevice::ConnectedState);
        _serialPort->clear(); // only possible after open
    } else {
        setError(_serialPort->errorString(), ModbusDevice::ConnectionError);
    }
    return (state() == ModbusDevice::ConnectedState);
}

///
/// \brief ModbusRtuClient::close
///
void ModbusRtuClient::close()
{
    if (state() == ModbusDevice::UnconnectedState)
        return;

    setState(ModbusDevice::ClosingState);

    if (_serialPort->isOpen())
        _serialPort->close();

    int numberOfAborts = 0;
    while (!_queue.isEmpty()) {
        // Finish each open reply and forget them
        ModbusClientPrivate::QueueElement elem = _queue.dequeue();
        if (!elem.reply.isNull()) {
            elem.reply->setError(ModbusDevice::ReplyAbortedError,
                                 QModbusClient::tr("Reply aborted due to connection closure."));
            numberOfAborts++;
        }
    }

    if (numberOfAborts > 0)
        qCDebug(QT_MODBUS_LOW) << "(RTU client) Aborted replies:" << numberOfAborts;

    setState(ModbusDevice::UnconnectedState);
}

///
/// \brief ModbusRtuClient::enqueueRequest
/// \param request
/// \param serverAddress
/// \param unit
/// \param type
/// \return
///
ModbusReply* ModbusRtuClient::enqueueRequest(int requestGroupId, const QModbusRequest &request, int serverAddress, const QModbusDataUnit &unit, ModbusReply::ReplyType type)
{
    auto reply = new ModbusReply(serverAddress == 0 ? ModbusReply::Broadcast : type, serverAddress, this);
    reply->setRequestGroupId(requestGroupId);

    QueueElement element(reply, request, unit, numberOfRetries() + 1);
    element.adu = QModbusSerialAdu::create(QModbusSerialAdu::Rtu, serverAddress, request);
    _queue.enqueue(element);

    scheduleNextRequest(_interFrameDelayMilliseconds);

    return reply;
}

///
/// \brief ModbusRtuClient::scheduleNextRequest
/// \param delay
///
void ModbusRtuClient::scheduleNextRequest(int delay)
{
    if (_state == Idle && !_queue.isEmpty()) {
        _state = WaitingForReplay;
        QTimer::singleShot(delay, this, [this]() { processQueue(); });
    }
}

///
/// \brief ModbusRtuClient::processQueue
///
void ModbusRtuClient::processQueue()
{
    _responseBuffer.clear();
    _serialPort->clear(QSerialPort::AllDirections);

    if (_queue.isEmpty())
        return;
    auto &current = _queue.first();

    if (current.reply.isNull()) {
        _queue.dequeue();
        _state = Idle;
        scheduleNextRequest(_interFrameDelayMilliseconds);
    } else {

        const auto msgReq = ModbusMessage::create(current.adu, ModbusMessage::Rtu, QDateTime::currentDateTime(), true);
        emit modbusRequest(current.reply->requestGroupId(), msgReq);

        current.bytesWritten = 0;
        current.numberOfRetries--;
        _serialPort->write(current.adu);

        qCDebug(QT_MODBUS) << "(RTU client) Sent Serial PDU:" << current.requestPdu;
        qCDebug(QT_MODBUS_LOW).noquote() << "(RTU client) Sent Serial ADU: 0x" + current.adu.toHex();
    }
}

///
/// \brief ModbusRtuClient::canMatchRequestAndResponse
/// \param response
/// \param sendingServer
/// \return
///
bool ModbusRtuClient::canMatchRequestAndResponse(const QModbusResponse &response, int sendingServer) const
{
    if (_queue.isEmpty())
        return false;
    const auto &current = _queue.first();

    if (current.reply.isNull())
        return false;   // reply deleted
    if (current.reply->serverAddress() != sendingServer)
        return false;   // server mismatch
    if (current.requestPdu.functionCode() != response.functionCode())
        return false;   // request for different function code
    return true;
}

///
/// \brief ModbusRtuClient::on_readyRead
///
void ModbusRtuClient::on_readyRead()
{
    _responseBuffer += _serialPort->read(_serialPort->bytesAvailable());
    qCDebug(QT_MODBUS_LOW) << "(RTU client) Response buffer:" << _responseBuffer.toHex();

    if (_responseBuffer.size() < 2) {
        qCDebug(QT_MODBUS) << "(RTU client) Modbus ADU not complete";
        return;
    }

    const QModbusSerialAdu tmpAdu(QModbusSerialAdu::Rtu, _responseBuffer);
    int pduSizeWithoutFcode = QModbusResponse::calculateDataSize(tmpAdu.pdu());
    if (pduSizeWithoutFcode < 0) {
        // wait for more data
        qCDebug(QT_MODBUS) << "(RTU client) Cannot calculate PDU size for function code:"
                           << tmpAdu.pdu().functionCode() << ", delaying pending frame";
        return;
    }

    // server address byte + function code byte + PDU size + 2 bytes CRC
    int aduSize = 2 + pduSizeWithoutFcode + 2;
    if (tmpAdu.rawSize() < aduSize) {
        qCDebug(QT_MODBUS) << "(RTU client) Incomplete ADU received, ignoring";

        if (!_queue.isEmpty() && !_queue.first().reply.isNull()) {
            const auto msg = ModbusMessage::create(tmpAdu.rawData(), ModbusMessage::Rtu, QDateTime::currentDateTime(), false);
            emit modbusResponse(_queue.first().reply->requestGroupId(), msg);
        }

        return;
    }

    if (_queue.isEmpty())
        return;
    auto &current = _queue.first();

    // Special case for Diagnostics:ReturnQueryData. The response has no
    // length indicator and is just a simple echo of what we have send.
    if (tmpAdu.pdu().functionCode() == QModbusPdu::Diagnostics) {
        const QModbusResponse response = tmpAdu.pdu();
        if (canMatchRequestAndResponse(response, tmpAdu.serverAddress())) {
            quint16 subCode = 0xffff;
            response.decodeData(&subCode);
            if (subCode == Diagnostics::ReturnQueryData) {
                if (response.data() != current.requestPdu.data())
                    return; // echo does not match request yet
                aduSize = 2 + response.dataSize() + 2;
                if (tmpAdu.rawSize() < aduSize)
                    return; // echo matches, probably checksum missing
            }
        }
    }

    const QModbusSerialAdu adu(QModbusSerialAdu::Rtu, _responseBuffer.left(aduSize));
    _responseBuffer.remove(0, aduSize);

    qCDebug(QT_MODBUS) << "(RTU client) Received ADU:" << adu.rawData().toHex();
    if (QT_MODBUS().isDebugEnabled() && !_responseBuffer.isEmpty())
        qCDebug(QT_MODBUS_LOW) << "(RTU client) Pending buffer:" << _responseBuffer.toHex();

    // check CRC
    if (!adu.matchingChecksum()) {
        qCWarning(QT_MODBUS) << "(RTU client) Discarding response with wrong CRC, received:"
                             << adu.checksum<quint16>() << ", calculated CRC:"
                             << QModbusSerialAdu::calculateCRC(adu.data(), adu.size());
        if (!current.reply.isNull()) {
            current.reply->addIntermediateError(ModbusDevice::ResponseCrcError);

            const auto msg = ModbusMessage::create(adu.rawData(), ModbusMessage::Rtu, QDateTime::currentDateTime(), false);
            emit modbusResponse(current.reply->requestGroupId(), msg);
        }
        return;
    }

    const QModbusResponse response = adu.pdu();
    if (!canMatchRequestAndResponse(response, adu.serverAddress())) {
        qCWarning(QT_MODBUS) << "(RTU client) Cannot match response with open request, "
                                "ignoring";
        if (!current.reply.isNull())
            current.reply->addIntermediateError(ModbusDevice::ResponseRequestMismatch);
        return;
    }

    _state = ProcessReply;
    _responseTimer.stop();
    current.m_timerId = INT_MIN;

    processQueueElement(response, ModbusMessage::Rtu, adu.serverAddress(), _queue.dequeue());

    _state = Idle;
    scheduleNextRequest(_interFrameDelayMilliseconds);
}

///
/// \brief ModbusRtuClient::on_aboutToClose
///
void ModbusRtuClient::on_aboutToClose()
{
    Q_ASSERT(state() == ModbusDevice::ClosingState);
    _responseTimer.stop();
}

///
/// \brief ModbusRtuClient::on_responseTimeout
/// \param timerId
///
void ModbusRtuClient::on_responseTimeout(int timerId)
{
    _responseTimer.stop();
    if (_state != State::WaitingForReplay || _queue.isEmpty())
        return;
    const auto &current = _queue.first();

    if (current.m_timerId != timerId)
        return;

    qCDebug(QT_MODBUS) << "(RTU client) Receive timeout:" << current.requestPdu;

    if (current.numberOfRetries <= 0) {
        auto item = _queue.dequeue();
        if (item.reply) {
            item.reply->setError(ModbusDevice::TimeoutError,
                                 QModbusClient::tr("Request timeout."));
        }
    }

    _state = Idle;
    scheduleNextRequest(_interFrameDelayMilliseconds);
}

///
/// \brief ModbusRtuClient::on_bytesWritten
/// \param bytes
///
void ModbusRtuClient::on_bytesWritten(qint64 bytes)
{
    if (_queue.isEmpty())
        return;
    auto &current = _queue.first();

    current.bytesWritten += bytes;
    if (current.bytesWritten != current.adu.size())
        return;

    qCDebug(QT_MODBUS) << "(RTU client) Send successful:" << current.requestPdu;

    if (!current.reply.isNull() && current.reply->type() == ModbusReply::Broadcast) {
        _state = ProcessReply;
        const int serverAddress = current.reply->serverAddress();
        processQueueElement({}, ModbusMessage::Rtu, serverAddress, _queue.dequeue());
        _state = Idle;
        scheduleNextRequest(_turnaroundDelay);
    } else {
        current.m_timerId = _responseTimer.start(timeout());
    }
}

///
/// \brief ModbusRtuClient::on_error
/// \param error
///
void ModbusRtuClient::on_error(QSerialPort::SerialPortError error)
{
    if (error == QSerialPort::NoError)
        return;

    qCDebug(QT_MODBUS) << "(RTU server) QSerialPort error:" << error
                       << (_serialPort ? _serialPort->errorString() : QString());

     switch (error) {
    case QSerialPort::DeviceNotFoundError:
        setError(QModbusDevice::tr("Referenced serial device does not exist."),
                    ModbusDevice::ConnectionError);
        break;
    case QSerialPort::PermissionError:
        setError(QModbusDevice::tr("Cannot open serial device due to permissions."),
                    ModbusDevice::ConnectionError);
        break;
    case QSerialPort::OpenError:
    case QSerialPort::NotOpenError:
        setError(QModbusDevice::tr("Cannot open serial device."),
                    ModbusDevice::ConnectionError);
        break;
    case QSerialPort::WriteError:
        setError(QModbusDevice::tr("Write error."), ModbusDevice::WriteError);
        break;
    case QSerialPort::ReadError:
        setError(QModbusDevice::tr("Read error."), ModbusDevice::ReadError);
        break;
    case QSerialPort::ResourceError:
        setError(QModbusDevice::tr("Resource error."), ModbusDevice::ConnectionError);
        break;
    case QSerialPort::UnsupportedOperationError:
        setError(QModbusDevice::tr("Device operation is not supported error."),
                    ModbusDevice::ConfigurationError);
        break;
    case QSerialPort::TimeoutError:
        setError(QModbusDevice::tr("Timeout error."), ModbusDevice::TimeoutError);
        break;
    case QSerialPort::UnknownError:
        setError(QModbusDevice::tr("Unknown error."), ModbusDevice::UnknownError);
        break;
    default:
        qCDebug(QT_MODBUS) << "(RTU server) Unhandled QSerialPort error" << error;
        break;
    }
}
